import lejos.nxt.*;
import lejos.robotics.navigation.*;


public class Square {
	
	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.6f;
	static double AXIS_LENGTH = 5.6f;
	
	public static void main(String[] args) throws InterruptedException {
		
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		
		// We set the parameters to smooth the trajectory:
		navigator.setRotateSpeed(100);
		navigator.setTravelSpeed(100);
		navigator.setAcceleration(200);
		
		Thread.sleep(1000);
		
		//Forward - stop - rotate
		for (int i = 0; i < 4; i++)
		{
			navigator.travel(SIDE_SIZE);
			navigator.rotate(90);
		}
		
		
		
		Button.waitForPress();
	}
}

